package com.yiren.dbaa.util;

import com.vividsolutions.jts.geom.Coordinate;
import com.vividsolutions.jts.geom.Geometry;
import com.vividsolutions.jts.geom.GeometryFactory;
import com.vividsolutions.jts.geom.LineString;
import com.vividsolutions.jts.geom.LinearRing;
import com.vividsolutions.jts.geom.MultiLineString;
import com.vividsolutions.jts.geom.Point;
import com.vividsolutions.jts.geom.Polygon;
import com.yiren.backstage.currency.bean.PointBean;
import com.yiren.backstage.currency.entity.TrajectoryPointEntity;
import com.yiren.backstage.currency.utils.GpsUtil;

import java.util.ArrayList;
import java.util.List;

/**
 * 几何运算工具
 */
public class JtsUtils {

    private GeometryFactory geometryFactory;

    public JtsUtils() {
        geometryFactory = new GeometryFactory();
    }

    /**
     * 创建点
     */
    public Coordinate createPoint(PointBean point) {
        return new Coordinate(point.getX(), point.getY());
    }

    /**
     * 创建几何点位
     */
    public Point createGeomPoint(PointBean point) {
        return geometryFactory.createPoint(createPoint(point));
    }

    /**
     * 两个点创建线
     */
    public LineString createLine(PointBean onePoint, PointBean twoPoint) {
        List<Coordinate> coordinates = new ArrayList<>();
        coordinates.add(createPoint(onePoint));
        coordinates.add(createPoint(twoPoint));
        return createLine(coordinates);
    }

    /**
     * 点位数组创建线
     */
    public LineString createLine(PointBean[] points) {
        List<Coordinate> coordinates = new ArrayList<>();
        for (int i = 0; i < points.length; i++) {
            coordinates.add(createPoint(points[i]));
        }
        return createLine(coordinates);
    }

    /**
     * 轨迹点位创建线
     */
    public LineString createLineByPoints(List<TrajectoryPointEntity> points) {
        List<Coordinate> coordinates = new ArrayList<>();
        for (int i = 0; i < points.size(); i++) {
            coordinates.add(createPoint(points.get(i).getPoint()));
        }
        return createLine(coordinates);
    }

    /**
     * 坐标点集合创建线
     */
    public LineString createLine(List<Coordinate> points) {
        Coordinate[] coords = (Coordinate[]) points.toArray(new Coordinate[points.size()]);
        if (coords.length > 1) {
            LineString line = geometryFactory.createLineString(coords);
            return line;
        }
        return null;
    }

    /**
     * 创建多条线
     */
    public MultiLineString createMLine() {
        Coordinate[] coords1 = new Coordinate[]{new Coordinate(2, 2), new Coordinate(2, 2)};
        LineString line1 = geometryFactory.createLineString(coords1);
        Coordinate[] coords2 = new Coordinate[]{new Coordinate(2, 2), new Coordinate(2, 2)};
        LineString line2 = geometryFactory.createLineString(coords2);
        LineString[] lineStrings = new LineString[2];
        lineStrings[0] = line1;
        lineStrings[1] = line2;
        MultiLineString ms = geometryFactory.createMultiLineString(lineStrings);
        return ms;
    }

    /**
     * 坐标点集合创建(多边形)
     */
    public Polygon createPolygonByWKT(List<Coordinate> points) {
        Polygon polygon = null;
        Coordinate[] coords = (Coordinate[]) points.toArray(new Coordinate[points.size()]);
        polygon = geometryFactory.createPolygon(coords);
        return polygon;
    }

    /**
     * 根据轨迹点位创建（多边形）
     *
     * @param points
     * @return
     */
    public Polygon createPolygonByPoints(List<TrajectoryPointEntity> points) {
        List<Coordinate> coordinates = new ArrayList<>();
        for (int i = 0; i < points.size(); i++) {
            coordinates.add(createPoint(points.get(i).getPoint()));
        }
        coordinates.add(createPoint(points.get(0).getPoint()));
        return createPolygonByWKT(coordinates);
    }

    /**
     * 创建一个圆，圆心(x,y) 半径(radius)
     */
    public Polygon createCircle(double x, double y, final double radius) {
        int SIDES = 32;//圆上面的点个数
        Coordinate coords[] = new Coordinate[SIDES + 1];
        for (int i = 0; i < SIDES; i++) {
            double angle = ((double) i / (double) SIDES) * Math.PI * 2.0;
            double dx = Math.cos(angle) * radius;
            double dy = Math.sin(angle) * radius;
            coords[i] = new Coordinate((double) x + dx, (double) y + dy);
        }
        coords[SIDES] = coords[0];
        LinearRing ring = geometryFactory.createLinearRing(coords);
        Polygon polygon = geometryFactory.createPolygon(ring, null);
        return polygon;
    }

    /**
     * 返回几何图形a指定距离内的多边形和多多边形
     */
    public Geometry bufferGeo(Geometry a, double distance) {
        return a.buffer(distance);
    }

    /**
     * 两个几何对象的交集
     */
    public Geometry intersectionGeo(Geometry a, Geometry b) {
        return a.intersection(b);
    }

    /**
     * 几何对象合并
     */
    public Geometry unionGeo(Geometry a, Geometry b) {
        return a.union(b);
    }

    /**
     * 在A几何对象中有的，但是B几何对象中没有
     */
    public Geometry differenceGeo(Geometry a, Geometry b) {
        return a.difference(b);
    }

    /**
     * 返回(A)与(B)中距离最近的两个点的距离
     */
    public double distanceGeo(Geometry a, Geometry b) {
        return a.distance(b);
    }

    /**
     * 几何形状A的线都在几何形状B内部
     */
    public boolean within(Geometry a, Geometry b) {
        return a.within(b);
    }

    /**
     * 几何形状B的线都在几何形状A内部（区别于内含）
     */
    public boolean contains(Geometry a, Geometry b) {
        return a.contains(b);
    }

    /**
     * 交叉 几何形状共享一些但不是所有的内部点。
     */
    public boolean crosses(Geometry a, Geometry b) {
        return a.crosses(b);
    }

    /**
     * 相交 几何形状至少有一个共有点（区别于脱节）
     */
    public boolean intersects(Geometry a, Geometry b) {
        return a.intersects(b);
    }

    /**
     * 两个点之间的间距
     */
    public double distance2Point(PointBean onePoint, PointBean twoPoint) {
        return Math.sqrt(Math.pow(twoPoint.getX() - onePoint.getX(), 2) + Math.pow(twoPoint.getY() - onePoint.getY(), 2));
    }

    /**
     * 获取一条线段相对于另一条线段是顺时针还是逆时针旋转
     * point1,point2为固定线段上两个点，point2为前面的点,线段a
     * point3,point4为改变线段上两个点，point4为前面的点,线段b
     */
    public boolean getLineRotateDirection(PointBean point1, PointBean point2, PointBean point3, PointBean point4) {
        float f = (point2.getX() - point1.getX()) * (point4.getY() - point3.getY()) - (point4.getX() - point3.getX()) * (point2.getY() - point1.getY());
        if (f >= 0) {   //b⃗在a⃗的逆时针方向
            return true;
        } else {        //b⃗在a⃗的顺时针方向
            return false;
        }
    }

    /**
     * 获取两条线段是否相交
     * a1,a2为线段1 && b1,b2为线段2
     * 判断车辆2点和24点是否风别在线段左右边
     */
    public boolean getTwoLineIntersect(PointBean a1, PointBean a2, PointBean b1, PointBean b2) {
        if (null == a1 || null == a2 || null == b1 || null == b2) {
            return false;
        }
        boolean aIsLeft = pointIsTrajectoryLeft(a1, b1, b2);
        boolean aIsLeft2 = pointIsTrajectoryLeft(a2, b1, b2);
//        boolean bIsLeft = pointIsTrajectoryLeft(b1, a1, a2);
//        boolean bIsLeft2 = pointIsTrajectoryLeft(b2, a1, a2);
        if ((aIsLeft == !aIsLeft2)) {
            return true;
        }
        return false;
    }

    /**
     * 判断点在线段的左边还是右边
     *
     * @param currentPoint 当前点位
     * @param oneTPoint    轨迹第一个点
     * @param twoTPoint    轨迹第二个
     */
    public boolean pointIsTrajectoryLeft(PointBean currentPoint, PointBean oneTPoint, PointBean twoTPoint) {
        if (currentPoint == null || oneTPoint == null || twoTPoint == null) {
            return false;
        }
        float f = (twoTPoint.getX() - oneTPoint.getX()) * (currentPoint.getY() - oneTPoint.getY()) - (currentPoint.getX() - oneTPoint.getX()) * (twoTPoint.getY() - oneTPoint.getY());
        if (f >= 0) {  //点位在线的左边
            return true;
        } else {       //点在线的右边
            return false;
        }
    }

    /**
     * 获取当前线段旋转固定角度度的线段
     *
     * @param point1 当前线段上的点
     * @param tanq   当前线段的斜率
     * @param degree 旋转角度
     * @param offset 偏移量
     */
    public PointBean[] getRotateDegreeLine(PointBean point1, double tanq, double degree, int offset) {
        PointBean[] points = new PointBean[2];
        double tan40 = Math.tan(Math.toRadians(degree));
        points[0] = new PointBean(point1.getX() + offset, (float) (offset * (tan40 + tanq) / (1 - tan40 * tanq) + point1.getY()));
        points[1] = new PointBean(point1.getX() - offset, (float) (-offset * (tan40 + tanq) / (1 - tan40 * tanq) + point1.getY()));
        return points;
    }

    /**
     * 修正车辆模型点位（用于排除测绘杆半径的影响）
     *
     * @param point0 原点
     * @param point1 y轴上的点
     * @param point2 任意点
     * @param radius 测绘杆半径
     */
    public double[] getAdjustPoint(PointBean point0, PointBean point1, PointBean point2, float radius) {
        double degree = getInCludeAngle(point0, point1, point2);
        double deltaX = radius * Math.sin(Math.toRadians(degree));
        double deltaY = radius * Math.cos(Math.toRadians(degree));
        return new double[]{deltaX, deltaY};
    }

    /**
     * 获取任意点与原点之间的连线 和 y轴之间的夹角
     *
     * @param point0 原点
     * @param point1 y轴上的点
     * @param point2 任意点
     * @return
     */
    public double getInCludeAngle(PointBean point0, PointBean point1, PointBean point2) {
        float tanAngle1 = Math.abs((point2.getX() - point0.getX()) / (point2.getY() - point0.getY()));
        float tanAngle2 = Math.abs((point1.getX() - point0.getX()) / (point1.getY() - point0.getY()));
        float tanAngle = Math.abs((tanAngle2 - tanAngle1) / (1 - tanAngle1 * tanAngle2));
        double degree = Math.toDegrees(Math.atan(tanAngle));
        return degree;
    }

    /**
     * 判断线段是否与多边形轨迹相交
     *
     * @param lines
     * @param points
     * @return
     */
    public boolean intersectsPolygon(PointBean[] lines, List<TrajectoryPointEntity> points) {
        LineString lineString1 = createLine(lines);
        LineString lineString2 = createLineByPoints(points);
        if (null != lineString1 && null != lineString2) {
            return lineString1.intersects(lineString2);
        }
        return false;
    }

    /**
     * 获取当前线段平行的线段
     *
     * @param point1 线段起点
     * @param point2 线段终点
     * @param offset 偏移量
     * @param isLeft 向左偏移还是向右偏移
     */
    public PointBean[] getParallelLines(PointBean point1, PointBean point2, float offset, boolean isLeft) {
        PointBean[] points = new PointBean[2];
        double tana = (point2.getY() - point1.getY()) / (point2.getX() - point1.getX());
        float dx = point2.getX() - point1.getX();
        float x, x2, y, y2;
        if (isLeft) {
            if (tana >= 0) {
                if (dx >= 0) {  //第一象限
                    x = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                } else {        //第三象限
                    x = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                }
            } else {
                if (dx >= 0) {  //第四象限
                    x = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                } else {        //第二象限
                    x = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                }
            }
        } else {
            if (tana >= 0) {
                if (dx >= 0) {  //第一象限
                    x = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                } else {        //第三象限
                    x = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                }
            } else {
                if (dx >= 0) {  //第四象限
                    x = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                } else {        //第二象限
                    x = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getX());
                }
            }
        }
        if (isLeft) {
            if (dx >= 0) {      //第一、四象限
                y = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getY());
            } else {            //第二、三象限
                y = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getY());
            }
        } else {
            if (dx >= 0) {      //第一、四象限
                y = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getY());
            } else {            //第二、三象限
                y = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getY());
            }
        }

        if (isLeft) {
            if (tana >= 0) {
                if (dx >= 0) {
                    x2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                } else {
                    x2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                }
            } else {
                if (dx >= 0) {
                    x2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                } else {
                    x2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                }
            }
        } else {
            if (tana >= 0) {
                if (dx >= 0) {
                    x2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                } else {
                    x2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                }
            } else {
                if (dx >= 0) {
                    x2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                } else {
                    x2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getX());
                }
            }
        }
        if (isLeft) {
            if (dx >= 0) {
                y2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getY());
            } else {
                y2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getY());
            }
        } else {
            if (dx >= 0) {
                y2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getY());
            } else {
                y2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getY());
            }
        }
        points[0] = new PointBean(x, y);
        points[1] = new PointBean(x2, y2);
        return points;
    }

    /**
     * 根据前面3个点算出第四个点（矩形或梯形）
     *
     * @return
     */
    public PointBean getPoint4ByFront3(PointBean point1, PointBean point2, PointBean point3, boolean isBaseCalibration) {
        //当3点与1,2点连线的垂足不在1,2点中间时，生成的平行线不平行，所以需要将1,2线延长
        //计算3点离1、2点的距离，生成1、2线的过3点的平行线
        float offset = (float) distanceGeo(createGeomPoint(point3), createLine(getExtendSegment(point1, point2, 10)));
        PointBean[] parallelLines = getParallelLines(point1, point2, offset, pointIsTrajectoryLeft(point3, point1, point2));
        PointBean point = new PointBean();
        if (null != parallelLines) {
            float x = parallelLines[0].x;
            float y = parallelLines[0].y;
            point.setPoint(x, y);
            point.setZ(point1.getZ());
            if (isBaseCalibration) {
                point.setPointJW(point3.getLon() + GpsUtil.getDistanceLon(x - point3.getX(), point3.getLat()), point3.getLat() + GpsUtil.getDistanceLat(y - point3.getY()));
                point.setHigh(point3.getHigh());
            }
            return point;
        }
        return null;
    }

    /**
     * 获取线段的延长线段
     *
     * @param point1 线段点1
     * @param point2 线段点2
     * @param offset 两边偏移距离
     * @return
     */
    public PointBean[] getExtendSegment(PointBean point1, PointBean point2, float offset) {
        PointBean[] points = new PointBean[2];
        double tana = (point2.getY() - point1.getY()) / (point2.getX() - point1.getX());
        float deltaY = point2.getY() - point1.getY();
        float x, x2, y, y2;
        if (tana >= 0) {
            if (deltaY >= 0) {
                x = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getX());
            } else {
                x = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getX());
            }
        } else {
            if (deltaY >= 0) {
                x = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getX());
            } else {
                x = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point1.getX());
            }
        }
        if (deltaY >= 0) {
            y = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getY());
        } else {
            y = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point1.getY());
        }

        if (tana >= 0) {
            if (deltaY >= 0) {
                x2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getX());
            } else {
                x2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getX());
            }
        } else {
            if (deltaY >= 0) {
                x2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getX());
            } else {
                x2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(tana, 2))) + point2.getX());
            }
        }
        if (deltaY >= 0) {
            y2 = (float) (Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getY());
        } else {
            y2 = (float) (-Math.sqrt(Math.pow(offset, 2) / (1 + Math.pow(1 / tana, 2))) + point2.getY());
        }
        points[0] = new PointBean(x, y);
        points[1] = new PointBean(x2, y2);
        return points;
    }

    /**
     * 获取两条线段相交的点
     */
    public static PointBean getCrossPoint(PointBean p1, PointBean p2, PointBean p3, PointBean p4) {
        double A1 = p1.getY() - p2.getY();
        double B1 = p2.getX() - p1.getX();
        double C1 = A1 * p1.getX() + B1 * p1.getY();
        double A2 = p3.getY() - p4.getY();
        double B2 = p4.getX() - p3.getX();
        double C2 = A2 * p3.getX() + B2 * p3.getY();
        double det_k = A1 * B2 - A2 * B1;
        if (Math.abs(det_k) < 0.00001) {
            return null;
        }
        double a = B2 / det_k;
        double b = -1 * B1 / det_k;
        double c = -1 * A2 / det_k;
        double d = A1 / det_k;
        double x = a * C1 + b * C2;
        double y = c * C1 + d * C2;
        return new PointBean((float) x, (float) y);
    }

    /**
     * 获取点3在线段1、2上的垂足
     */
    private PointBean getFoot(PointBean p1, PointBean p2, PointBean p3) {
        PointBean foot = new PointBean();
        float dx = p1.x - p2.x;
        float dy = p1.y - p2.y;

        float u = (p3.x - p1.x) * dx + (p3.y - p1.y) * dy;
        u /= dx * dx + dy * dy;

        foot.x = (int) (p1.x + u * dx);
        foot.y = (int) (p1.y + u * dy);

        float d = Math.abs((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.x - p2.y));
        float d1 = Math.abs((p1.x - foot.x) * (p1.x - foot.x) + (p1.y - foot.y) * (p1.x - foot.y));
        float d2 = Math.abs((p2.x - foot.x) * (p2.x - foot.x) + (p2.y - foot.y) * (p2.x - foot.y));

        if (d1 > d || d2 > d) {
            if (d1 > d2) return p2;
            else return p1;
        }
        return foot;
    }

    /**
     * 求点到线段的垂足
     * * @param pt1 线段上1点
     * * @param pt1 线段上2点
     * * @param point
     *
     * @return
     */
    public PointBean getPointFoot(PointBean pt1, PointBean pt2, PointBean point) {
        PointBean foot;
        if (pt2.getX() == pt1.getX()) {
            foot = new PointBean(pt2.getX(), point.getY());
        } else {
            //求线段 的斜率
            float k = (pt2.getY() - pt1.getY()) / (pt2.getX() - pt1.getX());
            float x = (float) ((Math.pow(k, 2) * pt1.getX() + k * (point.getY() - pt1.getY()) + point.getX()) / (Math.pow(k, 2) + 1));
            float y = k * (x - pt1.getX()) + pt1.getY();
            foot = new PointBean(x, y);
        }
        return foot;
    }

    /**
     * 判断轨迹上的点是否存在一个在指定线段的左边，一个在右边
     *
     * @param onePoint
     * @param twoPoint
     * @param trajectoryPoints
     */
    public List<TrajectoryPointEntity> getTrajectoryPoints(PointBean onePoint, PointBean twoPoint, List<TrajectoryPointEntity> trajectoryPoints) {
        if (null == onePoint || null == twoPoint) {
            return null;
        }
        List<TrajectoryPointEntity> points = new ArrayList<>();
        for (int i = 0; i < trajectoryPoints.size(); i++) {
            if ((i + 1) < trajectoryPoints.size()) {
                PointBean point1 = trajectoryPoints.get(i).getPoint();
                PointBean point2 = trajectoryPoints.get(i + 1).getPoint();
                if (pointIsTrajectoryLeft(point1, onePoint, twoPoint) != pointIsTrajectoryLeft(point2, onePoint, twoPoint)) {
                    points.add(trajectoryPoints.get(i));
                    points.add(trajectoryPoints.get(i + 1));
                    return points;
                }
            }
        }
        return null;
    }

    /**
     * 判断轨迹上的点是否存在一个在指定线段的左边，一个在右边
     *
     * @param onePoint
     * @param twoPoint
     * @param trajectoryPoints
     * @return
     */
    public List<TrajectoryPointEntity> getTrajectoryPoints(PointBean onePoint, PointBean twoPoint, List<TrajectoryPointEntity> trajectoryPoints, String headIndex) {
        if (null == onePoint || null == twoPoint) {
            return null;
        }
        List<TrajectoryPointEntity> points = new ArrayList<>();
        for (int i = 0; i < trajectoryPoints.size(); i++) {
            if ((i + 1) < trajectoryPoints.size()) {
                String head1 = trajectoryPoints.get(i).getOrderIndex().substring(0, 1);
                String head2 = trajectoryPoints.get(i + 1).getOrderIndex().substring(0, 1);
                PointBean point1 = trajectoryPoints.get(i).getPoint();
                PointBean point2 = trajectoryPoints.get(i + 1).getPoint();
                if (headIndex.equals(head1) && head1.equals(head2) && pointIsTrajectoryLeft(point1, onePoint, twoPoint) != pointIsTrajectoryLeft(point2, onePoint, twoPoint)) {
                    points.add(trajectoryPoints.get(i));
                    points.add(trajectoryPoints.get(i + 1));
                    if (points.size() == 4) {
                        return points;
                    }
                    headIndex = trajectoryPoints.get(trajectoryPoints.size() - 1).getOrderIndex().substring(0, 1);
                }
            }
        }
        return null;
    }

    /**
     * 获取B线段相对于A线段逆时针旋转之间的夹角（0，-360）
     *
     * @param A1
     * @param A2
     * @param B1
     * @param B2
     * @return
     */
    public float angleBetween2Lines(PointBean A1, PointBean A2, PointBean B1, PointBean B2) {
        float angle1 = (float) Math.atan2(A2.getY() - A1.getY(), A1.getX() - A2.getX());
        float angle2 = (float) Math.atan2(B2.getY() - B1.getY(), B1.getX() - B2.getX());
        float calculatedAngle = (float) Math.toDegrees(angle1 - angle2);
        if (calculatedAngle < 0) calculatedAngle += 360;
        return calculatedAngle;
    }

}
